from pylab import *

import scipy.misc
import scipy.ndimage

if __name__ == '__main__':




    phi00 = array([[ 0, 0, 1, 1, 1, 0, 0],
                   [ 0, 1, 1, 1, 1, 1, 0],
                   [ 1, 1, 1, 1, 1, 1, 1],
                   [ 1, 1, 1, 1, 1, 1, 1],
                   [ 1, 1, 1, 1, 1, 1, 1],
                   [ 0, 1, 1, 1, 1, 1, 0],
                   [ 0, 0, 1, 1, 1, 0, 0]]) / 37.


    phi01 = array([[      0, -0.0161, -0.0215, 0, 0.0215, 0.0161,      0],
                   [-0.0236, -0.0574, -0.0360, 0, 0.0360, 0.0574, 0.0236],
                   [-0.0648, -0.0724, -0.0564, 0, 0.0564, 0.0724, 0.0648],
                   [-0.0802, -0.0808, -0.0777, 0, 0.0777, 0.0808, 0.0802],
                   [-0.0648, -0.0724, -0.0564, 0, 0.0564, 0.0724, 0.0648],
                   [-0.0236, -0.0574, -0.0360, 0, 0.0360, 0.0574, 0.0236],
                   [      0, -0.0161, -0.0215, 0, 0.0215, 0.0161,      0]])
    phi11 = array([[      0, -0.0130, -0.0140, 0,  0.0140,  0.0130,      0],
                   [-0.0210, -0.0247,  0.0023, 0, -0.0023,  0.0247, 0.0210],
                   [-0.0419,  0.0050,  0.0432, 0, -0.0432, -0.0050, 0.0419],
                   [-0.0319,  0.0215,  0.0855, 0, -0.0855, -0.0215, 0.0319],
                   [-0.0419,  0.0050,  0.0432, 0, -0.0432, -0.0050, 0.0419],
                   [-0.0210, -0.0247,  0.0023, 0, -0.0023,  0.0247, 0.0210],
                   [      0, -0.0130, -0.0140, 0,  0.0140,  0.0130,      0]])

    phi10 = array([[     0,  0.0237,  0.0448,  0.0466,  0.0448,  0.0237,      0],
                   [0.0238,  0.0357, -0.0550, -0.0218, -0.0550,  0.0357, 0.0238],
                   [0.0448, -0.0550, -0.0623, -0.0903, -0.0623, -0.0550, 0.0448],
                   [0.0466, -0.0218, -0.0903, -0.0137, -0.0903, -0.0218, 0.0466],
                   [0.0448, -0.0550, -0.0623, -0.0903, -0.0623, -0.0550, 0.0448],
                   [0.0238,  0.0357, -0.0550, -0.0218, -0.0550,  0.0357, 0.0238],
                   [     0,  0.0237,  0.0448,  0.0466,  0.0448,  0.0237,      0]])

    phi20 = array([[     0,  0.0170,  0.0176,  0.0112,  0.0176,  0.0170,      0],
                   [0.0171, -0.0066, -0.0419, -0.0431, -0.0419, -0.0066, 0.0171],
                   [0.0176, -0.0419, -0.0147,  0.0306, -0.0147, -0.0419, 0.0176],
                   [0.0112, -0.0431,  0.0306,  0.0149,  0.0306, -0.0431, 0.0112],
                   [0.0176, -0.0419, -0.0147,  0.0306, -0.0147, -0.0419, 0.0176],
                   [0.0171, -0.0066, -0.0419, -0.0431, -0.0419, -0.0066, 0.0171],
                   [     0,  0.0170,  0.0176,  0.0112,  0.0176,  0.0170,      0]])


    dev=scipy.array([
        [ -0.003776, -0.010199, 0., 0.010199, 0.003776 ],
        [ -0.026786, -0.070844, 0., 0.070844, 0.026786 ],
        [ -0.046548, -0.122572, 0., 0.122572, 0.046548 ],
        [ -0.026786, -0.070844, 0., 0.070844, 0.026786 ],
        [ -0.003776, -0.010199, 0., 0.010199, 0.003776 ]
        ])


    # zernike_filter= np.array([
    #         [-00,-15,-19,00,19,15,00],
    #         [-22,-47,-23,00,23,47,22],
    #         [-57,-47,-23,00,23,47,57],
    #         [-70,-47,-23,00,23,47,70],
    #         [-57,-47,-23,00,23,47,57],
    #         [-22,-47,-23,00,23,47,22],
    #         [-00,-15,-19,00,19,15,00]])/1292.
    # zernike_filter= np.array([
    #         [-.0147, -.0469, 0, .0469, .0147],
    #         [-.0933, -.0640, 0, .0640, .0933],
    #         [-.1253, -.0640, 0, .0640, .1253],
    #         [-.0933, -.0640, 0, .0640, .0933],
    #         [-.0147, -.0469, 0, .0469, .0147]]) / 1.9368

    zernike_filter = array([[-104, -125, 0,  125,  104],
                 [-254,  563, 0, -563,  254],
                 [-075,  870, 0, -870,  075],
                 [-254,  563, 0, -563,  254],
                 [-104, -125, 0,  125,  104],
                 ]) * .1e-3



    ion()


    ex,ey = 380, 123

    #ll = scipy.misc.lena()
    ll = np.load('snp.npz')['arr_0']

    q = array([-.5,.5])


    figure(10)
    subplot(3,2,1)
    imshow(dev, extent=(1,5,1,5), vmin=-.15, vmax=.15)
    axis([0,6,6,0])
    subplot(3,2,3)
    imshow(phi01, vmin=-.15, vmax=.15)
    subplot(3,2,4)
    imshow(phi11, vmin=-.15, vmax=.15)
    subplot(3,2,5)
    imshow((2*phi01+phi11)/4, vmin=-.15, vmax=.15)
    subplot(3,2,6)
    imshow(zernike_filter, vmin=-.15, vmax=.15)
    # subplot(3,2,2)
    # imshow((2*phi01+phi11)/4 - zernike_filter, vmin=-.15, vmax=.15)
    


    figure(1)
    imshow(ll, cmap=cm.bone)
    figure(2)
    imshow(ll, cmap=cm.bone)

    P00 = scipy.ndimage.convolve(ll, phi00)
    P10 = scipy.ndimage.convolve(ll, phi10)
    P20 = scipy.ndimage.convolve(ll, phi20)
    P01r = scipy.ndimage.convolve(ll, phi01)
    P01i = scipy.ndimage.convolve(ll, phi01.T)
    P11r = scipy.ndimage.convolve(ll, phi11)
    P11i = scipy.ndimage.convolve(ll, phi11.T)

    P01 = sqrt(P01r*P01r+P01i*P01i)
    P11 = sqrt(P11r*P11r+P11i*P11i)

    Dx = scipy.ndimage.convolve(ll, dev)
    Dy = scipy.ndimage.convolve(ll, dev.T)


    Zr = scipy.ndimage.convolve(ll, zernike_filter)
    Zi = scipy.ndimage.convolve(ll, zernike_filter.T)

    # lims = [210,260,270,220]
    # lims = [0,41,41,0]
    lims = [15,25,25,15]

    p_l = ((4*P10+P20)/(2*P01+P11))*3.0/5.0
    p_k = (2*P01+P11)/ (2*(1-p_l**2)**(3.0/2.0))
    p_h = (P00 - p_k * (arcsin(1-p_l**2)**.5-p_l*(1-p_l**2)**.5))/pi

    edgels_ofmm = []
    edgels_zer = []
    edgels_dev = []

    for ex in range(lims[0],lims[1],1):
        for ey in range(lims[3],lims[2],1):
            cy = 2*P01i[ey,ex] + P11i[ey,ex]
            cx = 2*P01r[ey,ex] + P11r[ey,ex]
            nf = (cx*cx+cy*cy)**-.5
            cx *= nf
            cy *= nf



            p00 = P00[ey,ex]

            dx = Dx[ey,ex]
            dy = Dy[ey,ex]

            nf = (dx*dx+dy*dy)**-.5
            dx *= nf
            dy *= nf


            zy = Zi[ey,ex]
            zx = Zr[ey,ex]
            nf = (zx*zx+zy*zy)**-.5
            zx *= nf
            zy *= nf

            

            edgels_ofmm.append(cy)
            edgels_zer.append(zy)
            edgels_dev.append(dy)

            figure(1)
            plot(ex+q*cx, ey+q*cy, 'r-')
            figure(2)
            plot(ex+q*dx, ey+q*dy, 'r-')
    
    figure(1)
    axis(lims)
    figure(2)
    axis(lims)


    edgels_ofmm = array(edgels_ofmm)
    edgels_zer = array(edgels_zer)
    edgels_dev = array(edgels_dev)

    figure(4)
    plot(sort(np.abs(edgels_ofmm-.3)), mgrid[:edgels_ofmm.shape[0]])
    plot(sort(np.abs(edgels_zer-.3)), mgrid[:edgels_zer.shape[0]])
    plot(sort(np.abs(edgels_dev-.3)), mgrid[:edgels_ofmm.shape[0]])
    grid()


    # yy,xx = mgrid[:50,:50]

    # ddx = .5
    # ddy = sqrt(1-ddx**2)
    # ff = (ddx*xx+ddy*yy)

    # figure(3)
    # imshow(ff)

    # ex,ey = 11,9

    # snp = ff[ey-2:ey+3,ex-2:ex+3]

    # dx = (snp * dev).sum()
    # dy = (snp * dev.T).sum()
    # nf = (dx*dx+dy*dy)**-.5
    # dx *= nf
    # dy *= nf

    # plot(ex+4*q*dx, ey+4*q*dy, 'r-')

    # Dx = scipy.ndimage.convolve(ff, dev)
    # Dy = scipy.ndimage.convolve(ff, dev.T)

    # plot(ex+4*q*Dx[ex,ey], ey+4*q*Dy[ey,ey], 'g-')
